#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl_conversions/pcl_conversions.h>

using namespace std;
boost::shared_ptr<pcl::visualization::PCLVisualizer> pointCloudViewer(new pcl::visualization::PCLVisualizer("Live Point Cloud"));

ros::Publisher pointCloudPublisher;

typedef pcl::PointCloud<pcl::PointXYZ> XYZPointCloud;

void handlePointCloud(const sensor_msgs::PointCloud2ConstPtr& inputMsg) {
    pcl::PCLPointCloud2* rawCloud = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudContainer(rawCloud);
    pcl_conversions::toPCL(*inputMsg, *rawCloud);

    pointCloudPublisher.publish(*rawCloud);

    XYZPointCloud::Ptr pclCloud;
    pclCloud.reset(new XYZPointCloud);
    pcl::fromROSMsg(*inputMsg, *pclCloud);

    pointCloudViewer->removeAllPointClouds();
    pointCloudViewer->addPointCloud(pclCloud, "Live Point Cloud");
    pointCloudViewer->updatePointCloud(pclCloud, "Live Point Cloud");
    pointCloudViewer->spinOnce(0.001);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "point_cloud_processor");
    ros::NodeHandle nodeHandler;

    ros::Subscriber cloudSubscriber = nodeHandler.subscribe("/rslidar_points", 10, handlePointCloud);
    pointCloudPublisher = nodeHandler.advertise<pcl::PCLPointCloud2>("processed_output", 10);

    ros::spin();
}

